#include "BambooReel.h"
#include "WPILib.h"
#include "math.h"
#include "Timer.h"
#include "DigitalInput.h"

BambooReel::BambooReel(UINT32 MotorChannel, UINT32 SolenoidChannel, UINT32 SwitchSlot, UINT32 SwitchChannel, UINT32 aChannel, UINT32 bChannel, Joystick *stick)
{
	m_motor = new Jaguar(MotorChannel);
	m_stick = stick;
	m_state = kRest;
	m_Solenoid = new Solenoid(SolenoidChannel);
	m_switch = new DigitalInput(SwitchSlot, SwitchChannel);
	m_encoder = new Encoder(aChannel, bChannel, true);
	m_kickTime = 0;
}	
BambooReel::~BambooReel()
{
	delete m_motor;
	delete m_Solenoid;
	delete m_switch;
	delete m_encoder;
	delete m_stick;
}
void BambooReel::Teleop()
{
	
	double rotations;
	switch (m_state)
		{
		case kRest:
			if (m_switch->Get() == true)
			{
				if (m_stick->GetButton((Joystick::ButtonType)KickButton));
				{
				m_Solenoid->Set(true);
				m_kickTime = GetTime();
				m_state = kKick;
				}
			}
			else
				m_state = kPull;
		case kKick:
			if (GetTime() - m_kickTime >= 500)
			{
				m_kickTime = 0;
				m_encoder->Start();
				m_state = kPull;
			}
		case kPull:
			if (m_switch->Get() == true)
			{
				m_Solenoid->Set(false);
				m_motor->Set(0);
				rotations = m_encoder->Get();
				m_encoder->Reset();
				m_encoder->Start();
				m_state = kUnwind;
			}
			else
				m_motor->Set(1);
		case kUnwind:
			m_motor->Set(-1);
			if (fabs(m_encoder->Get()) >= fabs(rotations))
			{
				m_motor->Set(0);
				m_encoder->Reset();
				m_state = kRest;
			}
		}
}
